# -*- encoding: utf-8 -*-
"""
@File    :   allFuncTest.py

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/10 下午10:26 

@Author :   赵方国        
"""
from naoqi import ALProxy
from cmath import pi
import time
import almath
from obstacle import correctAngle
from walkToObstacle import lineCorrect
from ImagProcess import ImagProgressHSV
import vision_definitions
import cv2 as cv
import numpy as np


# ----------------------------------------------------------------------------------------
# 各个函数的测试文件新代码的试验文件！！！主程序不需要

# ----------------------------------------------------------------------------------------

def angleTest(robotIP, PORT):
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    angleINIT = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")

    while True:
        motionProxy.moveToward(0.5, 0, 0, lower_steps)
        angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
        # correctAngle(robotIP, PORT, angleINIT)
        print angle


def correctAngleTEST(robotIP, PORT, angleINIT):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    print "current angle:" + str(angle)
    angleREcorrect = (angle - angleINIT)
    print "angleRECORRECT:" + str(angleREcorrect)
    if abs(angleREcorrect) < 1 * pi / 180:
        print "do not need to correct"
        return True
    if abs(angleREcorrect) > pi:
        if angleREcorrect > 0:
            angleCorrect = abs(angle - angleINIT) - pi - pi / 2
        else:
            angleCorrect = -(abs(angle - angleINIT) - pi - pi / 2)
        motionProxy.moveTo(0, 0, -angleCorrect)
        print "大于 pi"
        print "turn to " + str(- angleCorrect)
        return True

    if 50 \
            * pi / 180 < abs(angleREcorrect) < 49 * pi / 180:
        motionProxy.moveTo(0, 0, -(angle - angleINIT))
        time.sleep(1)
        print 'turn'
        print str(-(angle - angleINIT))
        return True
    if abs(angleREcorrect) > 93 * pi / 180 or ((87 * pi / 180) > abs(angleREcorrect) > (50 * pi / 180)):
        if angleREcorrect < 0:
            motionProxy.moveTo(0, 0, (abs(angleREcorrect) - 90 * pi / 180))
        else:
            motionProxy.moveTo(0, 0, -(abs(angleREcorrect) - 90 * pi / 180))
        print "大于90度，校准"
        print 'turn to ' + str((abs(angleREcorrect) - 90 * pi / 180))
        return True


def obstacleHENGZHEZOUTEST(robotIP, PORT, angleINIT):
    """
    进行从右边绕过障碍物的动作，采用横着走的方式
    @param robotIP: 机器人IP
    @param PORT: 9559
    @param angleINIT:初始角度值用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the left obstacle")

    # 进行角度的校准
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0, 0.55, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)

    # motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)


def obstacleHENGZHEZOU2TEST(robotIP, PORT, angleINIT):
    """
    进行从左边绕过障碍物的动作，采用横着走的方式
    @param robotIP: 机器人IP
    @param PORT: 9559
    @param angleINIT:初始角度值用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the left obstacle")

    # 进行角度的校准
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0, -0.55, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)

    # motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)


def distanceTEST(robotIP, PORT, distanceX, distanceY, distanceZ):
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    robotMove = 0
    while robotMove < 0.30:
        motionProxy.moveToward(0.5, 0, 0, lower_steps)
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove = almath.Pose2D.toVector(robotMove)
        robotMove = robotMove[0]
        print "Robot Move:", robotMove
    motionProxy.stopMove()
    return True


# 测试后即可放在obstacle.py内
def obstacleHENGZHEZOUwithLINECORRECTTEST(robotIP, PORT, angleINIT):
    """
    进行从右边绕过障碍物的动作，采用横着走的方式,加上了场线校准
    @param robotIP: 机器人IP
    @param PORT: 9559
    @param angleINIT:初始角度值用于校准
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.say("Now is the left obstacle")

    # 进行角度的校准
    # angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    # lineCorrect(robotIP, PORT, angleINIT)

    correctAngle(robotIP, PORT, angleINIT)

    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    robotMove = 0
    while robotMove < 0.55:
        motionProxy.moveToward(0.1, 0, 0, lower_steps)
        endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        print "Robot Move:", robotMove
        correctAngle(robotIP, PORT, angleINIT)
        lineCorrect(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)
    # correctAngle(robotIP, PORT, angleINIT)

    # motionProxy.moveTo(0.55, 0, 0, smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    # 绕开障碍物
    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0, 0, (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.3, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, (-pi / 2), smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)


def getImagTEST(IP, PORT, cameraID, flag, name):
    """
    测试函数！！！连续输出坐标信息

    :param IP: 机器人IP
    :param PORT: 9559
    :param cameraID: 0为上摄像头，1为下摄像头
    :param name: 相机名称，一般由随机数生成
    :return: 图片一张
    """
    camProxy = ALProxy("ALVideoDevice", IP, PORT)
    resolution = vision_definitions.kQVGA
    colorSpace = vision_definitions.kBGRColorSpace
    fps = 20
    print "Is camera opened ?", camProxy.isCameraOpen(1)
    print "getting images in remote"
    nameID = camProxy.subscribe(str(name), resolution, colorSpace, fps)
    camProxy.setActiveCamera(cameraID)
    # data = [90, 0, 0, 0]
    i = 0
    while True:
        img = camProxy.getImageRemote(nameID)
        imagHeader0 = np.array(img[6])
        imagHeader = map(ord, img[6])
        camProxy.releaseImage(nameID)
        imagHeader = np.reshape(imagHeader, [240, 320, 3])
        img = np.uint8(imagHeader)
        # cv.imshow(str(name), img)
        data1 = ImagProgressHSV(img, flag)
        print data1
        print data1[2] * data1[3]
        cv.waitKey(1)


if __name__ == '__main__':
    robotIP = "169.254.250.216"

    PORT = 9559
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)

    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    time.sleep(2)
    angleINIT = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")

    # angleTest(robotIP, PORT)
    # obstacleHENGZHEZOU(robotIP, PORT, angleINIT)
    # name = str(random.randint(0, 1000))
    # print getImagTEST(robotIP, 9559, 1, "blue", name)
    # # print getImagTEST(robotIP, 9559, 1, "red", name)
    # # print getImagTEST(robotIP, 9559, 1, "yellow", name)
    # cv.waitKey(1)
    # distanceTEST(robotIP, PORT, 0, -0.3, 0)
    getImagTEST(robotIP, PORT, 1, "blue", "aiegaiewlgwhair")
    # distanceTEST(robotIP, PORT, 0, 0.55, 0)
